classdef doubleJoint <handle
    %重开！！！！注意这一次一水平为0角度点，接着以逆时针为正方向
    
    properties
        m   = 0.5;         % mass of robot
        n_factor;         %注意n用一个值
        
        l   = 0.1;         % length of robot
        g   = 9.81;        % Gravity Constant
        B1  = 2e-2;         % 阻尼系数
        Br  = 4.1e-4;
        Ir   = 1.19e-5;   % MOI of motor ,I1自己算
        I1 = 5e-3;
        
        
        Im;          %等效Im
        Bm;   %等效阻尼系数
        
        err = 0.05;
        f_m = 0.5;
        f_l = 0.1;
        f_Im;
        %以顺时针为正方向
        is_g_enable;    %0 for no applying
        state;  % pos& vel & acc,三维，是减速前的
        
        joint_state;
       
        
        n_group = 2;
    end
    
    methods
        %状态量为2行3列，其余全部为2*1列向量
        function obj = doubleJoint(origin_state,is_g_enable,n_factor)
            obj.joint_state = origin_state;
            obj.state = zeros(2,3);
            obj.is_g_enable = is_g_enable;
            obj.n_factor = n_factor;
            
            %线性部分相关的量
            obj.f_m = obj.m* (1-obj.err);
            obj.f_l = obj.l* (1-obj.err);
            
            
            m11 = 3*obj.m*obj.l^2/(obj.n_factor^2)+ obj.Ir;
            m22 = obj.m * obj.l^2/(obj.n_factor^2) + obj.Ir;
            obj.Im = [m11 0 ;0 m22];
            
            m11 = 3*obj.f_m*obj.f_l^2/(obj.n_factor^2);
            m22 = obj.f_m * obj.f_l^2/(obj.n_factor^2) + obj.Ir;
            obj.f_Im = [m11 0; 0 m22];
            
            
            b11 = obj.B1/(obj.n_factor^2)+obj.Br;
            obj.Bm = [b11 0; 0 b11];
            
        end
        
        function reset_state(obj,origin_state)
            %仅限初始位置全为0的情况使用
            obj.joint_state = origin_state;
            obj.state = obj.joint_state.*obj.n_factor;
        end
        
        
        function torque_info = inverse_dynamic(obj,is_fake,joint_state,state)
            %用于计算理论力矩
            if(is_fake == 0)
                m  = obj.m;
                l  = obj.l;
                Im = obj.Im;
            else
                m = obj.f_m;
                l = obj.f_l;
                Im = obj.f_Im;
            end
            m11 = m*l^2* 2* cos(joint_state(2,1)) /(obj.n_factor^2) + obj.Ir;
            m12 = m*l^2*(1 + cos(joint_state(2,1)))   /(obj.n_factor^2);
            m21 = m*l^2*(1 + cos(joint_state(2,1)))   /(obj.n_factor^2);
            m22 = 0;
            M = [m11 m12; m21 m22];
            
            v12 = -( m *l^2* (2*joint_state(1,2)+ joint_state(2,2)) ...
                *sin(joint_state(2,1))) / (obj.n_factor^2);
            v21 =( m *l^2*joint_state(2,1) *sin(joint_state(2,1)) )/ (obj.n_factor^2);
            V = [0 v12; v21 0];
            
            g11 = (2*m*obj.g*l*cos(joint_state(1,1))+m...
                *obj.g*l*cos(joint_state(1,1)+joint_state(2,1)))/obj.n_factor;
            g21 = (m...
                *obj.g*l*cos(joint_state(1,1)+joint_state(2,1)))/obj.n_factor;
            G = [g11 ;g21];
            
            dynamic_torque = M *state(:,3) + V* state(:,2) +G.*obj.is_g_enable;
            
            
            linear_torque = obj.Bm* state(:,2) + Im*state(:,3);
            
            all = dynamic_torque+ linear_torque;
            
            torque_info = [all,linear_torque,dynamic_torque];
        end
        
        
         function [M,V,G] = inverse_dynamic_matrix(obj,is_fake,joint_state)
            %用于计算理论力矩,重开后更新
            if(is_fake == 0)
                m  = obj.m;
                l  = obj.l;
                Im = obj.Im;
            else
                m = obj.f_m;
                l = obj.f_l;
                Im = obj.f_Im;
            end
            m11 = m*l^2*(3 + 2* cos(joint_state(2,1))) /(obj.n_factor^2)+obj.Ir;
            m12 = m*l^2*(1 + cos(joint_state(2,1)))   /(obj.n_factor^2);
            m21 = m*l^2*(1 + cos(joint_state(2,1)))   /(obj.n_factor^2);
            m22 = m*l^2 /(obj.n_factor^2) + obj.Ir;
            M = [m11 m12; m21 m22];
            
            v12 = -( m *l^2* (2*joint_state(1,2)+ joint_state(2,2)) ...
                *sin(joint_state(2,1))) / (obj.n_factor^2);
            v21 =( m *l^2*joint_state(2,1) *sin(joint_state(2,1)) )/ (obj.n_factor^2);
            V = [0 v12; v21 0];
            
            g11 = (2*m*obj.g*l*cos(joint_state(1,1))+m...
                *obj.g*l*cos(joint_state(1,1)+joint_state(2,1)))/obj.n_factor;
            g21 = (m...
                *obj.g*l*cos(joint_state(1,1)+joint_state(2,1)))/obj.n_factor;
            G = [g11 ;g21];
        end
 
        function [M,V,B,G] = joint_inverse_dynamic_matrix(obj,is_fake,joint_state)
            %用于计算理论力矩
            if(is_fake == 0)
                m  = obj.m;
                l  = obj.l;
            else
                m = obj.f_m;
                l = obj.f_l;

            end
            
            theta1 = joint_state(1,1);
            theta2 = joint_state(2,1);
            omega1 = joint_state(1,2);
            omega2 = joint_state(2,2);
            
            
            M = [m*(l^2)*(3 + 2*cos(theta2)) + obj.Ir*(obj.n_factor^2),...
                m*(l^2)*(1 + cos(theta2));...
                m*(l^2)*(1 + cos(theta2)),...
                m*(l^2) + obj.Ir*(obj.n_factor^2)];
            
            
            V = [0, (m * (l^2) * (2*omega1 + omega2)*sin(theta2));
                (m * (l^2) * omega1 * sin(theta2)), 0];
            
            B = [obj.B1+obj.Br*(obj.n_factor^2), 0;
                0, obj.B1+obj.Br*(obj.n_factor^2)];
            
            G = [(2*m*obj.g*l*cos(theta1) + m*obj.g*l*cos(theta1+theta2));
                m*obj.g*l*cos(theta1+theta2)];    
        end
        
        function disable_g(obj)
            obj.is_g_enable = zeros(2,1);
        end
        

        function state_info = simu_loop(obj,input_tau,simu_period)
            %输入的控制值，即，tau，应该是负数
%             tmp = obj.inverse_dynamic(0,obj.joint_state,obj.state);
            %重新使用,直接获得矩阵来求
            
            [M,V,G] = obj.inverse_dynamic_matrix(0,obj.joint_state);

            obj.state(:,3)  = M\(input_tau - G.*obj.is_g_enable -...
                V* obj.state(:,2) - obj.Bm* obj.state(:,2)); %B用-纯粹是因为角度方向规定问题
            obj.state(:,2) = obj.state(:,2)+ obj.state(:,3)*simu_period;
            obj.state(:,1) = obj.state(:,1)+ obj.state(:,2)*simu_period;
            
            obj.joint_state(:,2:end) = obj.state(:,2:end)./obj.n_factor;
            obj.joint_state(:,1) = obj.joint_state(:,1)+obj.joint_state(:,2)*simu_period;
            state_info = obj.joint_state;
        end
        
        function cart_value = get_cart_value(obj,jacobi)
            %需要传入雅可比矩阵，就只算速度，不要加速度
            %重开后更新!逆时针为正！
            cart_vel = jacobi*obj.joint_state(:,2);
            th1 = abs(obj.joint_state(1,1));
            th2 = abs(obj.joint_state(2,1));
            
            cart_pos = [obj.l*cos(th1)+obj.l*cos(th2-th1) ...
                ;obj.l*sin(th1)- obj.l*sin(th2-th1)];
                     
            cart_value = [cart_pos, cart_vel];            
        end
        
        function J= get_jacobi(obj,is_fake)
            % Compute the forward kinematics of a 2-R robot ,fake means
            % calculating by measured value
            % take horizonal pos as a zero pos
            % return jacobi
            if(is_fake == 1)
                l =  obj.f_l;
            else
                l = obj.l;
            end
%             th1 = abs(obj.joint_state(1,1));
%             th2 = abs(obj.joint_state(2,1));
%             J = [-l*(sin(th1)+sin(th1+th2))   -l*sin(th1+th2);...
%                 l*(cos(th1)+cos(th1+th2))  l*cos(th1+th2)];
            theta1 = obj.joint_state(1,1);
            theta2 = obj.joint_state(2,1);
            J = [-l*sin(theta1)-l*sin(theta1+theta2), -l*sin(theta1+theta2);
      l*cos(theta1)+l*cos(theta1+theta2),  l*cos(theta1+theta2)];
        
        end
        
        function d_J= get_dot_jacobi(obj,is_fake)
            % Compute the forward kinematics of a 2-R robot ,fake means
            % calculating by measured value
            % take horizonal pos as a zero pos
            % return the d of jacobi
            if(is_fake == 1)
                l =  obj.f_l;
            else
                l = obj.l;
            end
            theta1 = obj.joint_state(1,1);
            theta2 = obj.joint_state(2,1);
            omega1 = obj.joint_state(1,2);
            omega2 = obj.joint_state(2,2);
            d_J = [-( l*cos(theta1)*omega1+l*cos(theta1+theta2)*(omega1+omega2)),...
                -( l*cos(theta1+theta2)*(omega1+omega2));...
       -( l*sin(theta1)*omega1+l*sin(theta1+theta2)*(omega1+omega2)),...
       -( l*sin(theta1+theta2)*(omega1+omega2))];
        end
        
        
    end
    
    
end